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ABSTRACT 


This  report  describes  the  satellite  tracking  Kalman  filter  implemented 
at  the  M.I.T.  Lincoln  laboratory  Firepond  Infrared  Research  Facility  at 
Westford,  MA.  The  filter  estimates  a six  dimensional  state  vector  for  mount 
direction  from  satellite  observations.  These  observations  can  consist  of 
range,  elevation,  azimuth,  and  range  rate;  and  under  push-button  control,  can 
be  selected  from  among  the  available  Firepond  detectors  (IR  and  optical)  and 
the  Millstone  radar  across  the  road. 

Radar  polar  coordinates  are  used  throughout  and,  in  particular,  both 
the  estimates  and  the  equations  of  motion  are  in  these  coordinates.  The 
filter  is  fully  coupled  in  the  sense  that  every  measurement  improves  every 
estimate.  For  example,  angle  measurements  improve  range  and  Doppler  estimates, 
and  conversely.  Serial  processing  of  simultaneous  measurements  is  employed. 
This  eliminates  the  need  for  matrix  inversion,  facilitates  handling  of  missing 
data  points,  requires  less  storage,  and  is  computationally  faster. 

A detailed  mathematical  description  of  the  filter  is  included  along 
with  some  typical  satellite  tracking  results. 
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I . INTRODUCTION 

A Kalman  filter  for  satellite  direction  has  been  designed,  implemented, 

and  in  routine  mission  operation  at  the  M.I.T.  Lincoln  Laboratory  Firepond 

1 

Infrared  Research  Facility  in  Westford,  Mass.  The  filter  accepts  pre-smoothed 
observations  every  tenth  second  and  estimates  an  improved  state  vector  from  a 
predicted  state  vector  and  these  observations.  The  predicted  state  vector  is 
based  on  the  equations  of  motion.  Radar  polar  coordinates  are  used  throughout 
and  the  state  vector  is  six  dimensional  with  components  R,  E,  A,  R,  E,  A (range, 
elevation,  azimuth,  and  their  rates  respectively).  The  observation  vector  can 
be  up  to  four  dimensional  with  components  R,  E,A,  R.  These  components  are  pro- 
cessed sequentially,  with  time  increment  At  = 0.  This  facilitates  the  handling 
of  bad  or  missing  data  points  and  avoids  matrix  inversion.  Under  push-button 
control,  the  four  (or  less)  observation  components  can  be  selected  from  the  fol- 
lowing data  sources: 

a.  Millstone  R,  E,  A,  R 

b.  upper  visible  E,  A 

c.  lower  visible  E,  A 

d.  IR  R,  F,  A,  R 

These  can  be  mixed  in  any  way  with  the  exception  that  both  angles  must  be  from 
one  source.  For  example,  the  filter  can  accept  Millstone  range,  lower  visible 
angles,  and  IR  Doppler.  If  no  observations  are  selected  or  available,  the  fil- 
ter does  a predict  (i.e.,  coast)  cycle  without  estimation.  The  filter  is  fully 
coupled  in  the  sense  that  every  measurement  component  improves  the  estimation 
of  every  state  vector  component.  For  example,  angle  measurements  improve  range 
and  Doppler  estimates,  and  conversely. 

All  known  biases  (mount  misalignment,  refraction,  etc.)  are  removed  from 
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observations  before  input  to  the  Kalman  filter.  Based  on  these  true  observa- 
tions the  filter  estimates  a state  vector,  extrapolates  it  to  the  next  tenth 
second,  again  accounts  for  biases,  and  makes  available  a predicted  state  vector 
which,  at  the  option  of  the  test  director,  can  be  used  to  direct  the  mount. 

The  Kalman  track  residuals  are  displayed  (CRT)  and  recorded  (strip  chart)  for 
real-time  viewing  to  help  judge  the  quality  of  the  Kalman  track.  The  observa- 
tions and  filter  estimates  are  also  recorded  on  magnetic  tape  for  post-mission 
analysis. 

The  filter  has  been  successfully  used  in  a variety  of  missions.  Some 
typical  results  are  shown  in  Section  TV. 

1 1 . PREPROCESSOR 

1 . Firepond  Data 

The  function  of  the  Firepond  data  preprocessor  is  to  reduce  data  rates  to 
something  the  Kalman  filter  can  handle  in  the  available  time.  Recursive  linear 
least  squares  smoothing  is  performed  on  data  between  tenth  second  markers. 

Smoothed  values  are  updated  to  make  them  valid  at  the  tenth  second  markers  on 
or  immediately  following  the  last  sample  in  the  smoothing  interval.  The  smooth- 
ing algorithm  used  is  that  due  to  N.  Levine  reported  in  Ref.  5 and,  as  herein 
applied,  assumes  equally- spaced,  equal ly-weighted  samples. 

The  data  samples  fed  to  this  smoothing  algorithm  are  formed  as  indicated 
below  and  the  smoothing  algorithm  itself  is  outlined  in  Appendix  D. 
a.  Range 

An  algorithm  which  computes  range  track  offsets  from  early/late  gate  values 
resides  in  the  real  time  program  but  is  not  presently  used.  Instead,  uncorrected 
range  encoder  values  at  the  PRI  rate  (250  pps  maximum)  are  smoothed,  updated,  con- 
verted to  meters,  and  fed  to  the  Kalman  filter  every  tenth  second. 
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b.  Range  Rate 

Doppler  synthesizer  frequencies  and  filter  bank  Doppler  offsets  are  avail- 
able at  the  PRI  rate.  These  are  combined,  smoothed,  updated,  corrected  for 
relativistic  and  refraction  effects,  converted  to  m/sec,  and  fed  to  the  Kalman 
filter  every  tenth  second. 

c.  Angles 

The  50  pps  angle  track  error  components,  according  to  tracker  type,  are 
indicated  below. 

Tracker  Error  Components,  50  pps 

IR  Monopulse,  scan  mirror,  transit  time  mirror 

Lower  Visible  LV  detector,  scan  mirror,  transit  time  mirror 
Upper  Visible  UV  detector 

The  error  components  for  the  selected  tracker  are  combined,  smoothed,  updated, 
and  added  to  refraction  corrected  angle  encoder  values  at  the  tenth  second 
level.  In  the  case  of  the  lower  visible  tracker,  the  incremental  refraction 
mirror  correction  is  also  added  at  the  tenth  second  level.  The  results  are 
converted  to  radians  and  fed  to  the  Kalman  filter  every  tenth  second. 

d.  Updating 

The  tenth-second  updating  is  done  after  the  data  samples  are  formed  and 
smoothed  as  indicated  above.  Smoothed  values,  X,  are  linearly  extrapolated  to 
the  next  tenth  second  (indicated  by  the  subscript  ten)  as: 

X1Q  - X ♦ XAT  . 

The  rates  X are  approximated  from  the  Kalman  filter  estimated  rates  at  the  pre- 
vious tenth  second.  The  time  increments  AT  are  indicated  below  for  R,  E,  A, 
and  R. 
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ATr  * At  - R/C 

ATg,  ATa  * At  ♦ R/C  for  upper  visible 

* At  ♦ 2R/C  for  lower  visible  and  IR 

AT.  * At  - K/C  ♦ PRi 
R 

where  At  * time  between  last  datum  in  the  smoothing  interval 
and  the  next  even  tenth  second 
R/C  * transit  time  to  target 
PRI  * one  pulse  repetition  interval 


The  transit  time  and  PRI  terms  are  required  by  the  hardware  to  make  the  samples 
valid  at  target  illumination  time. 

2.  Millstone  Data 

The  Millstone  radar  observations  (R^,  E^  R^)  are  fully  corrected  and 
are  available  at  15  pps.  These  are  reduced  to  10  pps  by  accepting  data  only  at 
the  last  time  in  a tenth  second  interval.  These  are  then  referenced  to  Firepond 
and  fed  to  the  Kalman  filter  every  tenth  second. 

The  positional  components  (R^  E^,  are  first  put  in  radar  rectangular 
coordinates  (X^,  YM>  ZM)  and  then  converted  to  Firepond  rectangular  coordinates 
by  the  usual  rotation  and  translation. 
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The  rotation  matrix  [A]  is  prestored  in  the  real  time  program.  The  translation 
vector  (B^,  By,  B z)  is  also  prestored  (but  with  opposite  sign  sense).  The  Firepond- 
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referenced  rectangular  components  are  then  put  in  Firepond  polar  coordinates 
(R,  E,  A). 

The  range  rate  component  is  referenced  to  Firepond  as  indicated  in 
Appendiz  E.  The  transformation  of  the  associated  measurement  accuracy  weightings 
to  Firepond  coordinates  is  neglected  since  the  sites  are  so  close. 

III.  THE  KAIMAN  FILTER 

1.  Introduction 

Included  herein  is  a mathematical  description  of  the  Kalman  filter  as  it 
is  presently  implemented  on  the  Real  Time  Program  at  Firepond.  The  filter  al- 
gorithm is  a modification  and  extension  of  that  described  in  Reference  1.  The 
block  diagram  in  Figure  1 presents  a convenient  overview  of  the  whole  process. 
This  is  followed  by  a description  of  each  step  in  more  mathematical  detail. 

The  input  observations  to  the  filter  consist  of  the  tenth-second  smoothed 
and  corrected  R,  E,  A,  and  R samples.  Also  required  are  the  covariance  matrix 
M of  these  measurements,  an  initial  state  vector  and  its  covariance  matrix. 

The  algorithm  then  consists  of  a process  of  prediction  and  estimation  as  in- 
dicated in  Figure  1.  At  each  point  in  time  the  estimation  requires  a predicted 
state  vector  (and  its  convariance  matrix)  along  with  an  observation  vector, 
while  the  prediction  requires  an  estimate  of  the  last  state  vector  (and  its 
covariance  matrix)  and  the  equations  of  motion. 

State  vector  estimates  are  converted  to  Cartesian  coordinates  and  placed 
in  the  RTP  target  file  for  possible  laser  mount  directing. 

2.  Equations  of  Motion 

The  equations  of  motion  in  radar  polar  coordinates  are  shown  in  Appendix  A. 
They  appear  therein  as  three  coupled  second  order  differential  equations.  These 
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Fig.  1.  Firepond  Kalman  Filter. 
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can  he  compactly  written  as  the  first  order  vector  differential  equation 

x = f(x),  x(tQ)  = (lj 

where  x is  the  state  vector  with  components  R,  E,  A,  R,  E,  A,  and  f is  a six- 
dimensional vector- valued  function.  The  equation  (1)  and  other  items  to  follow 
are  written  out  in  more  explicit  detail  in  paragraph  5. 

3.  Model 

The  assumed  system  model  is  represented  by  the  pair  of  vector  equations 
(all  vectors  are  column  vectors) 

x = f(x)  + u (2) 


%c  * ^ 


where 


x is  the  6-dimensional  state  vector 

is  the  4-dimensional  observation  vector  at  time  t^ 

u is  a 6-dimensional  state  noise  vector  with  zero-mean,  independent 

Gaussian  samples  and  covariance  matrix  P = E[u,  u1]  ' 

v^  is  a 6-dimensional  observation  noise  vector  with  zero-mean,  independent 

T 

Gaussian  samples  and  covariance  matrix  M = E[v,  v ] 

H is  a 4 x 6 matrix  which  transforms  state  coordinates  to  observation 
coordinates. 

4 . Recursion  Algorithm 

The  Kalman  filter  recursion  algorithm  consists  of  a prediction  ar.d  an  es- 
timation. The  notation  is  used  to  denote  the  state  vector  estimate  at 

time  t^  based  on  all  measurements  through  t^  The  prediction  consists  of 
predicting  the  state  ^ and  its  covariance  matrix  ^ from  the  previous 
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estimates  al°n&  with  the  equations  of  motion  and  their  par- 

tial derivatives.  The  estimation  consists  of  combining  the  predictions 
Sk/k  1 w^t^1  t*ie  current  measurement  to  obtain  the  improved  estimates 
Sk/k  va^d  at  t^,  the  time  of  the  current  measurement.  This  is  described  in 
more  detail  below. 

a.  Partial  Derivatives 

The  required  partial  derivatives  are  indicated  below  in  compact  vector 
notation.  They  are  presented  more  explicitly  in  paragraph  5 and  in  full 
detail  in  Appendix  B. 

Differentiating  the  equations  of  motion  (1)  with  respect  to  time  we  obtain 
i = f (x) 


v . 9*  i « l£  f 
£ Tx  - 3x  ± 

h/k-1  = Ak-(^k/k-l) 


(3) 


where 


vl.  „ 

~ £ " ^k/k-l 

Differentiating  with  respect  to  we  obtain 

i = f(x) 


9xj(_  1 V<JtJ 


<£> 


9f  9a 
9- 


where 


d / 9* \ _ 9X_  3$ 

3t\3Xk-K  32S  3*k-l 

VAk  ♦k 

3ik 

<^k  = 4>(tk,  2 g^— 


(4) 


VI 
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b.  Prediction 


Using  (3)  in  a second  order  Taylor  expansion  we  obtain  the  state  vector 


prediction 


The  covariance  matrix  of  the  state  estimate  is  predicted  as 


Using  (4)  in  a first  order  Taylor  expansion  we  obtain 


I at  each  step  the  transition  matrix  <J>,  required  in  (6)  is 


computed  as 


c.  Estimation 


The  estimation  algorithm  follows 


n—H  |M»| 


where 


v 

-k/k-1  is  the  state  vector  estimate  at  time  t^  based  on  all 
measurements  through  time  t^  j. 

Sk/k-l  *s  t^'e  covariance  ^trix  (6  x 6)  of  the  estimate 

Wj_  is  the  weighting  or  gain  matrix  (6  x 4) 

T 

M is  the  (4  x 4)  measurement  noise  covariance  matrix  E[v,  v 1. 

The  present  implementation  assumes  that  the  measurement  noise  covariance  M does 
not  change  with  time.  If  later  desired  this  restriction  can  be  easily  removed. 

5.  Vectors  and  Matrices 

This  paragraph  contains  some  of  the  vectors  and  matrices  used  above  in  the 
more  explicit  and  specialized  form  that  they  appear  in  the  computer  implementation. 


. ~ 
m 4 


* (K,  E,  A,  RJ^  = observation  vector 
. . . T 

x * [R,  E,  A,  R,  E,  A]  = state  vector 


f(x)  = [fA  (x),  f2(x) f6(x)J 


(P.,  E,  A,  R(x),  E(x) , A(x)  J 
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*k 


* \-l st 


= transition  matrix 


Note  that  the  transition  matrix  ^ is  fully  coupled  except  for  linearization 
approximations . 

The  state  driving  noise  matrix  used  is 


0 0 


0 0 


0 0 0 0 


0 0 0 0 


0 0 c^(R)  0 0 

0 0 0 o*.(E)  0 

IN 


0 0 0 0 0 ojJ(A) 


The  non-zero  elements  are  computed  from  stored  values  of  oN  * a^//To  according 
to  data  source  as  indicated  below. 


Data  Source 

o*(R) , m/s 

* • 

oN(E) , prad/s 

* • 

on(A) , prad/s 

I 

Millstone 

.01 

.10 

.10 

Upper  Visible 

— 

.01 

.01 

Lower  Visible 

— 

.001 

.001 

1 

1 

IR 


.0001 


.001 


.001 


S is  the  6x6  state  estimate  covariance  matrix.  The  diagonal  terms  are  the 
variances  of  the  estimates. 


>n  * o2(R) 

S22  * 

S33  ’ °2<A> 

'44  = °2(R) 

S55  • °2® 

S66  ■ o2(A) 

The  off-diagonal  elements  are  symmetric  and  are  the  covariances  of  the 
estimates. 


S12 

= cov(R,E) 

S23 

= cov(E,A) 

S34  = cov(A.R) 

S45  = cov(R,E) 

S13 

= cov(R,A) 

S24 

= :ov(E,R) 

S35  = cov(A,E) 

S46  = cov(R,A) 

S14 

= cov(R,R) 

S25 

= cov(E,E) 

S36  = cov(A,A) 

S15 

* cov(R,E) 

S26 

= cov(E,A) 

S16 

* cov(R,A) 

The  lc**er  triangular  elements  are  set  equal  to  their  synmetric  elements  to  save 
computation. 

It  is  worth  noting  that,  in  the  above,  H takes  on  the  particularly  simple 

form  indicated  because  the  state  variables  are  in  the  same  coordinates  as  the 

observations.  If  this  were  not  true  H would  become  more  complex.  For  exanple, 

if  the  state  variables  were  expressed  in  radar -centered  rectangular  coordinates 

(x,  y,  z),  the  observations  would  be  nonlinear  functions  of  the  state. 

R * R(x,  y,  z) 

E «*  E(x,  y,  z) 

A - A(x,  y,  z) 

R - R(x,  y,  z,  x,  y,  z) 
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Linearization  approximations  would  then  be  required  and  lead  to  the  considerably 
more  complex  H below. 


9R 

aR 

3R 

ET 

5y 

Tz 

at 

3E 

3E 

Sx 

5y 

dZ 

3A 

aA 

9A 

ax 

ay 

3z 

3R 

aR 

3R 

3x 

ay 

3z 

0 

0 

0 

0 

0 

0 

0 

0 

0 

3R 

3R 

3R 

— r 

— V 

— r 

3x 

3y 

3z 

19) 


The  indicated  partials  are  written  in  full  in  Reference  2. 

6.  Data  Editing 

Dynamic  data  editing  is  performed  by  the  filter.  Data  which  deviate  from 
predicted  values  by  more  than  dynamically  computed  threshold  values  are  tagged 
as  bad  and  deleted  from  the  estimation  process.  The  threshold  values  are  com- 
puted as 

5o  + 2j^ 

where  ou  is  the  measurement  accuracy  for  the  particular  sensor  and  a is 
M 

initially  the  a p^Uofu.  state  vector  accuracy  and  subsequently  the  dynamically 
computed  state  vector  estimation  accuracy.  The  pre-assigned  accuracies  are  as 
indicated  below. 
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Sensor 

R(m) 

E(pr) 

A(pr) 

R(m/s) 

aa  pfUo^U 

All 

5,000 

100,000 

100,000 

1 

500 

J 

IR 

200 

10 

10 

1 

0.2  i 
f 

UV 

35 

35 

j 

LV 

20 

20 

1 

1 

I 

1 

m 

1,350 

350 

350 

1 

i 

3.5 

The  action  of  the  dynamic  editing  is  to  initially  accept  almost  all  data  and 
to  become  more  selective  as  the  filter  estimates  improve  with  track  length. 
Initially,  a » ofl  and  is  large  conpared  to  oM  so  that  threshold 

values  are  approximately  5 o^  p^0^-  After  a few  seconds  of  track,  a becomes 
small  conpared  to  o^,  so  that  the  threshold  values  become  approximately  2 o^. 


For  example,  lower  visible  data  are  initially  edited  with  a threshold  of 
approximately  500,000  urad  and,  after  a long  track,  with  a threshold  of 
approximately  40  yrad. 

i • 
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IV.  SCME  RESULTS 


The  Firepond  Kalman  filter  reported  here  was  initially  implemented  with 
Millstone  data  and  later  with  Firepond  visible  and  IR  data.  In  this  section 
we  present  some  results  for  two  satellite  track  missions  which  indicate  the 
kind  of  results  obtained  with  lower  visible  and  IR  Doppler  data. 

' 1.  Mission  76079.0036 

This  was  a Kalman  track  with  lower  visible  angles  only  on  object  5398 
(LCS4)  performed  on  18  March  1976.  The  tracking  crew  consisted  of  L.  Swezey, 

R.  Capes,  and  S.  Catalano.  This  was  a fifteen  minute  satellite  pass  with  peak 
elevation  of  48.3°.  The  object  was  acquired  at  8°  above  the  horizon,  put  into 
Kalman  directing  mode  at  25°  ascending,  and  held  in  the  lower  TV  reticle  down 
to  8°  for  about  nine  minutes.  The  scan  and  transit  time  servos  were  on  but  the 
incremental  refraction  servo  was  not  operational  and  left  off. 

A post-mission  orbit  fit  was  done  on  about  8.5  minutes  of  the  lower  visible 
angles  with  star  calibration  corrections  applied.  Kalman  angle  estimates  were 
within  ± 30  prad  of  the  orbit  values.  To  minimize  the  effect  of  not  having 
applied  incremental  refraction  corrections  to  the  visible  angle  data,  the  orbit 
fit  was  repeated  with  only  the  data  above  40°  elevation  (about  three  minutes  of 
data)  where  incremental  refraction  errors  are  less  than  7 prad.  the  Kalman 
angle  estimates  now  agreed  with  orbit  values  to  within  about  ± 7.5  prad  in 
azimuth  and  t 5.0  prad  in  elevation.  Figures  2a  and  2b  show  the  corrected 
lower  visible  angle  observations  fed  to  the  filter  and  the  corresponding  Kalman 
estimates.  Zero  levels  represent  the  fitted  orbit  values.  These  results  do 
not  preclude  the  possibility  of  linear  biases  in  the  system  since  the  orbit 
fit  would  adjust  to  such  biases  over  this  short  an  orbital  excursion. 


ft 
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AZIMUTH  (tog) 


ELEVATION  (dag) 


Fig.  2a.  Data  Residuals  (Orbit  fit  - LV  observations),  76079.0036,  LCS-4 


AZIMUTH  (dtgl 


ELEVATION  ( dtg I 


Fig.  2b.  Estimate  Residuals  (Orbit  fit  - Kalman  estimates),  76079.0036,  LCS-4 


2.  Mission  76098.0011 


I 


This  was  a Kalman  track  using  lower  visible  angles  and  IR  Doppler  and  was 
performed  on  GEOS- III  on  7 April  1976.  It  was  a 14  minute  pass  with  peak  eleva- 
tion of  63.8°.  The  tracking  crew  consisted  of  R.  Capes,  L.DiPalma,  J.  Linder, 

R.  McPherson,  L.  Swezey,  and  R.  Teoste.  The  object  was  acquired  in  lower  visible 
angles  from  nominals  and  by  about  32°  elevation  (going  up)  the  filter  was  in  good 
track  with  this  angles-only  data.  At  about  41°  IR  Doppler  hits  commenced,  ini- 
tially intermittent  and  later  steady.  The  filter  iirmediately  accepted  these  and 
continued  the  track  with  this  IR  Doppler  data  in  addition  to  the  lower  visible 
data.  At  about  54°,  the  mount  was  put  in  Kalman  directing  mode  and  remained  in 
this  mode  to  the  end  of  the  pass,  holding  the  IR  beam  on  the  object  through  peak 
elevation,  down  to  about  28°  elevation,  with  long  duration  IR  Doppler  returns 


for  about  four  minutes  in  essentially  "hands  off"  mode. 

An  orbit  fit  was  performed  on  the  basis  of  the  angle  and  Doppler  measure- 
ments. Figures  3a  and  3b  show  the  Kalman  observations  and  estimates  relative  to 
the  fitted  orbit.  All  measurements  include  corrections  for  servo  errors  (scan, 
transit  time,  and  incremental  refraction),  angle  and  Doppler  tracker  errors,  and 
star  calibration  misalignments.  The  azimuth  residuals  are  contained  within  a 
spread  of  about  50  urad  while  the  elevation  residuals  are  contained  within  a spread 
of  about  100  prad.  Some  of  the  funny  shapes  of  these  curves  are  probably  due  to 
the  star  calibration  corrections  which  were  still  in  the  process  cf  being  improved. 
The  Doppler  residuals  are  contained  within  a spread  of  about  0.5  m/s.  The  dis- 
continuity of  about  0.2  m/s  occurs  at  peak  elevation  and  was  probably  due  to  a 
system  bias. 
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Equations  of  Motion 

The  equations  of  motion  in  this  appendix  are  taken  from  Reference  3 
with  the  drag  terms  (containing  p)  deleted.  An  ellipsoidal  Earth  (equatorial 
radius  a and  flattening  f)  is  assumed  and  the  following  symbols  are  used. 

U = geodetic  site  latitude 
Mc  = geocentric  site  latitude 
= geocentric  target  latitude 
Rc  = Earth  radius  to  "foot"  of  radar  (see  Fig.  A-l) 
f = Earth  flattening  = 298.25  = .00335289  (dimensionless) 
e = Earth  eccentricity  [1  - e2  = (1  - f)2] 
a = Earth  equatorial  radius  = 6378145m 
u)  = Earth  rotation  rate  = 7.292  x 105  radians/sec 
CM  = product  of  gravitational  constant  and  Earth  mass  = 

3.985768  x 1014  m3/sec2 

J = Earth  second  gravitational  harmonic  = 1.62  x 10  3 (dimensionless) 
The  equations  of  motion  then  consist  of  the  following  set  of  three  coupled 
differential  equations. 

R=R(E2+A2cos2E)  - 2uC£eVa-£aVe)  - u>2(£Rr  sin0-rR)  ♦ ^ + g^R 
E=^2RE+RA2sinEcosE+2u)(^AVR-CRVA)  - w2UE  r sin*-rE)  - - g^J 

A*RcosE^ 2 (RAcosE-RAEs inE)  + 2w(^RVE  ^VR)  + w2(£Ar  sin<j>-rA)  - ~k-  - g^A) 
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where 


= cos  y cos  E cos  A + sin  y sin  E 
= -cosy  sin  E cos  A + sin  y cos  E 
= - cos  y sin  A 

vr  = R,  VE  = RE,  VA  = RA  cos  E 

V - (Vr2  * Ve2  * VA2)  »'2 

rR  = R + Q^sin  E - C^I^cos  E cos  A 

rE  = Ql^c  cos  ^ + Q2Rcsin  ^ cos  A 

rA  = Q2Rcsin  A 

Qj  = cos  (y  - yc)  + H/Rc 

Q2  = sin  (y  - yc) 

r = RC((R/RC)2  + 2(R/RC)  (QjSin  E - Q2cos  E cos  A)  * Q * + Q22}1/2 

gr  - ^ a ♦ J(f-)2  (1  ' 5 sin2<J>} 

gS  = '^2^  J(f^2  sin  ♦ 

sin  <t>  = I (R  cos  E cos  A cos  y + (H  + R sin  E)  sin  y + Rc  sin  yc) 

_2n  _2\  \<  ~ -1/2 

Rc  ■[  - — T ? 1 - a(l  - f)  11  - f(2  - f)cos2y  1 

c 1 1 - e^cos2  ^ c 

yc  * tan'1  ((1  - e2)  tan  y) 
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Fig.  A-l.  Earth- Sensor-Target  Geometry. 
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APPENDIX  B 

Partial  Derivatives 

This  section  presents  in  full  detail  the  A-matrix  partials  indicated  in 
paragraph  5.  The  elements  of  the  upper  half  A-matrix  are  trivial  and  already 
shown.  Therefore,  only  the  lower  half  A-matrix  elements  are  presented  here. 
The  notation  of  Appendix  A is  adhered  to  and  the  ellipsoidal  Earth  model  is 
retained. 

The  following  auxiliary  partials  are  first  required. 


3rR 

= Rc(Qi  cosE  + Q2  s^nE  cosA) » 


E 

jg-  = -RJQj  sinE  - Q2  cosE  cosA) , 


3rA 

W = RcQ2  COsA  ‘ 


Utilizing  these  auxiliary  partials,  the  lower  half  A-matrix  partials  are  as 
indicated  below. 


*rR 

W ' Rc«2  COSE  51nA 


^rE 

W~  = 'Rc^2  sinE  sinA 
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3R 

3E 


= -2kA“  sinE  cosE  + 2ojRA  (Cp  sinE  + £R  cosE) 
- w2  [cE  (RCr  - r sin*)  - 

[«»■■?] 


■ n 

r 


SR 

A43  S 3A  = 2w^A  VA  SinE  ' VE  C0SlJ  cosA) 


2 ^rR 

u cosE  (R£R  + r sin*)  - 


] 


A _ 3R  n 

A44  = -T  = 0 

™ 3R 


A45  5 f ■ 2<VE  * “«A> 


A46  = f ■ 2 cosE  <VA  ’ “*%> 
3A 
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_ aA  2AE 


2o) 


62  ' W 


Tr 


cos  E R cos  F, 
2 

R cos“E 


j-  (Vg  sinu  + VR  cosvi  cosA) 


^ 2 R cosE  + ^A  r * rA^  s*JlE] 

? ii[#)(r)  ■(?)“] 


*w  = SI  ■■  irasr  <VE C0SE  * vr  sinE> 


2 r 2 3rAl 

— I C R cosE  - r sin<|>  cosy  cosA  + I 

R cosE  A 3AJ 

i !!a1 

R cosE  3A  I 


*%hfe'2 

r 


V - !r  - - rksE  <A  cosE ' “V 


9E 


A65  - % “ 5S5-F  'A  sinE  - “V 


A66ii4.  2(EtarE-/) 


I- 

li 


. 
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APPENDIX  C 


! 


Serial  Estimation 

At  this  point  it  is  convenient  to  drop  the  double  subscript  notation 
of  the  estimation  algorithm  (8)  and  re-write  it  as 


W - S(-)HT[H  S(-)HT  ♦ Mf 1 
x(+)  = x(-)  + w[y  - H x(-)l 
S(+)  * S(-)  - W H S(-) 


(1) 


The  first  of  the  above  equations  requires  the  inversion  of  a 4 x 4 
matrix.  Many  matrix  inversion  schemes  have  been  proposed  and  used.  Originally, 
the  Firepond  Kalman  filter  used  an  extended  (for  doppler)  version  of  the 
scheme  suggested  in  Reference  I which  involves  inversion  after  factorization 
into  diagonal  matrices.  This  worked  but  was  cumbersome  in  many  respects. 

For  example,  if  one  or  more  components  of  the  observation  vector  were  missing, 
it  was  necessary  to  insert  fictitious  components  with  very  low  weights  so  as 
to  make  their  effect  insignificant.  The  assigning  of  these  low  weights  then 
became  a source  of  concern  as  to  whether  they  would  result  in  an  ill-conditioned 
matrix  that  might  behave  badly  upon  inversion.  Another  approach  might  have 
been  to  reformulate  the  algorithm  for  each  specific  situation  (i.e.  ,3x3 
inversion  for  one  missing  component,  etc.).  This  seemed  even  more  awkward. 

The  technique*  that  is  currently  used  avoids  the  above  difficulties. 

It  consists  in  regarding  the  four  simultaneously  occurring  components  of  the 
observation  vector  as  having  occurred  serially  over  a time  interval  tat  * 0. 

Then  instead  of  one  pass  through  the  algorithm  (1),  requiring  inversion  of  a 


*First  suggested  to  the  author  in  Reference  4,  p.  304,  305. 
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4x4  matrix,  a pass  is  made  for  each  observation  component,  each  pass 
requiring  only  the  trivial  inversion  of  a 1 x 1 matrix.  More  explicitly  four 
passes  are  made  through  the  algorithm  with  y,  H,  and  M as  indicated  below. 

1.  y « R,  H = [1,  0,  0,  0,  0,  0],  M - c^(R) 

2.  y - E,  H = [0,  1,  0,  0,  0,  0],  M = oJ(E) 

3.  y = A,  H = [0,  0,  1,  0,  0,  0],  M = OjJ(A) 

4.  y = R,  H = [0,  0,  0,  1,  0,  0],  M = o^(R) 


If  an  observation  component  is  missing  or  bad,  the  pass  for  that  component 
is  merely  omitted.  Each  pass  results  in  a six  dimensional  column  weight 
vector  W which  is  applied  to  the  old  state  vector  x(-)  and  covariance  matrix 
S(-)  to  obtain  the  new  ones,  x(+),  S(+).  On  the  pass  for  the  next  observa- 
tion component,  the  new  x(+),  S(+)  replace  the  old  x(-),  S(-). 

A simplified  version  of  the  FORTRAN  code  for  processing  four  simula- 
taneously  occurring  observation  components  through  the  algorithm  (1), 
in  the  described  serial  estimation  mode,  is  shown  below.  The  indicated  FORTRAN 
array  allocation  is  assumed. 


x(6)  = 

y(4) 

D(4) 


SI(MA(4) 
SP  (6,  6) 
SM  (6,  6) 


x(-)  on  entry  = predicted  state  at  observation  time 
x(+)  on  exit  = estimated  state  at  observation  time 
y,  observation  vector 

Data  flags  array,  one  flag  for  each  component  of  y,  with 
a flag  of  1 for  observations  which  are  to  be  included  in 
the  estimation. 

c^j (yi)  * assumed  measurement  accuracies  (standard  deviations) 
S(+),  the  new  covariance  matrix 
S(-),  the  old  covariance  matrix 


30 


k 


DO  30  K = 1,  4 
RES  = Y(K)  - X(K) 

IF  (D(K)  .NE.  1)  GO  TO  30 

TERM  = l.DO/(SM(K,  K)  ♦ SIGMA  (K)**2) 

DO  10  I = 1,  6 
W = SM(I,  K) *TF.RM 
X(I)  = X(I)  + W*RES 
DO  10  J - 1,  6 

10  SP(I,  J)  = SM(I,  J)  - W*SM(K,  J) 

DO  20  M = 1,  6 

DO  20  N = 1,  6 

20  SM(M,  N)  = SP(M,  N) 

30  CCNTINUE 

The  above  code  is  for  illustration  only.  The  actual  FIREPOND 
implementation  is  close  to  the  above  but  includes  modifications  which 
improve  the  computational  speed  (e.g. , taking  advantage  of  the  symmetry 
of  the  covariance  matrix) . 

The  serial  estimation  implementation  of  (1)  gives  the  same  answers 
as  the  originally  used  4x4  matrix  inversion  approach.  However,  the  serial 
estimation  approach  is  superior  on  several  counts. 

1.  It  does  not  require  a literature  search  for  the  best  matrix 
inversion  scheme. 

2.  It  requires  less  storage.  The  above  code  is  very  compact  and 
no  storage  is  required  for  a matrix  inversion  routine. 

3.  It  is  considerably  easier  to  implement  and  more  flexible  in  the 
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handling  of  missing  or  bad  observation  components. 

4.  It  is  computationally  faster.  This  is  due  to  the  use  of  radar 
polar  coordinates,  since  in  these  coordinates  the  matrix  H 
takes  on  a particularly  simple  form  (and  in  fact  does  not  even 
appear  in  the  above  code).  However,  in  other  coordinates,  H 
takes  on  the  complex  form  (9)  which  would  add  considerably  to 
the  computation. 
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Appendix  D 

Pre- smoothing  Algorithm 

The  pre-smoothing  algorithm  used  here  is  that  due  to  N.  Levine  reported 
in  reference  5.  As  used  herein,  it  performs  recursive  linear  least  squares 
smoothing  on  equally- spaced,  equally- weighted  data  (with  possible  missing 
points)  and  gives  estimates  of  position  and  velocity  valid  .it  the  last  data 
point.  The  algorithm  is  outlined  below. 

The  following  definitions  are  used: 

x = the  nth  observation 
n 

$ = its  predicted  value  based  on  previous  n-i  observations 
*n 

x = its  estimate  after  n observations 
n 

wn  * 1 for  a good  data  point  and  0 for  a bad  or  missing  data  point 

u * xx  where  t is  the  constant  data  interval 

0 * predicted  value  of  u based  on  n - 1 observations 
n n 

U * estimated  value  of  y based  on  n observations 
n n 

Mathematically  the  algorithm  can  be  written  as  follows: 

Initial  conditions:  Fj  * w^,  - Hj  * Jj  - 0 

*1  * *i»  Uj  * 0 
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Recursion  (n  > 1):  Fr  = Fn_^  + 


G - G , + F , 
n n-1  n-1 


H = H . + 2G  , + F , 
n n-1  n-1  n-1 


J = J . + w H 
n n-1  n n 


a = w H /J  , 6 = w G /J 
n n n n’  n n n'  n 


*n  ' xn-l  * “n-1-  °n  ' “ n-1 


x = £ + a (x  - x) 

n n n n n 


“n  - °n  * 6n  (xn  ' V 


Program -wise,  the  computation  is  more  efficiently  organized  as  indicated  below 
where  the  notation  A B means  A is  replaced  by  B. 

Initial  conditions:  * w^,  G * Hj  * J =0 


X1  = xl’  yi  = 0 


Recursion  (done  in  the  indicated  order) 


H 

■4- 

H 

♦ 

2G 

n 

n 

n 

G 

*4- 

G 

+ 

F 

n 

n 

n 

F 

-4- 

F 

+ 

w 

n 

n 

n 

J 

•4- 

J 

4- 

w H 

n 

n 

n l 

■4- 


X «-  X + u 

n n n 


Ax  •*-  (x  - x ) 
n n n' 


x ■*-  x + w Ax  H /J 
n n n n n n 


U p + w Ax  G /J 
n n n n n n 


In  this  form  previous  values  can  share  the  same  locations  as  present  values. 

All  subscripts  are  therefore  the  same  and  can  in  fact  he  dropped.  The  quan- 
tity Axn  is  the  difference  between  observed  and  predicted  value.  If  it  exceeds 
certain  thresholds  (90  km,  500  m/s,  10°  for  range,  doppler,  and  angle,  respec- 
tively) the  corresponding  observation  is  deleted  from  the  smoothing  process 
and  in  effect  replaced  by  its  predicted  value  by  setting  Ax^  = 0 in  the  last 
two  steps  above.  This  editing  is  intended  to  guard  against  gross  glitch-type 
errors  in  the  data.  A count  of  the  number  of  good  observations  in  the  tenth 
second  smoothing  process  is  maintained. 
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APPENDIX  E 


Millstone  Doppler  Coordinate  Correction 

Millstone  range  rate  is  converted  to  Fi repond -referenced  range  rate 

• AAA 

R as  indicated  below.  Referring  to  the  figure,  let  R,  E,  A be  Firepond  radar 
polar  unit  vectors.  Then  by  vector  definitions  we  have: 


OBJECT 


FIREPOND 


MILLSTONE 


R = RR 


B = BRR  + BeE  + BaA 

v = VrR  + vEE  ♦ vAA 
K.  = R - B 


= (R  - BJ  R - BpE  - B.A 


V * % 

Since  R..  = — — — , we  have 
m IX 


[vr  <R  - br>  ■ vebe  - Va  ] . 


Noting  that.VR  = R,  the  Firepond -referenced  range  rate,  and  solving,  we  get 

• Vm  * Ve  * bava 

• • • • 

In  the  above  Vf.  = RE,  VA  = RA  cos  E.  The  required  angle  rates  E,  A are  taken 

as  the  Kalman  filter  Firepond-referenced  angle  rate  estimates.  The  baseline 
ve^*or  B is  the  translation  vector  of  Section  IT-2  with  Firepond  rectangular 
components  in  meters  known  as: 

Bx  - 117.000,  By  = -15.098,  Bz  = 16.901 
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The  required  baseline  components  in  radar-polar  coordinates  are  obtained  as: 


BR  = BXCOS  E sin  A + By  cos  E cos  A ♦ B7  sin  E 
BE  = -BX  sin  E sin  A * By  sin  E cos  A + Bz  cos  E 
Ba  = Bx  cos  A - By  sin  A 


R,  E,  A are  the  Fi repond -referenced  Millstone  measurements 
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ThU  report  Jescrlbes  the  eeielllte  tricking  Kalman  filter  Implemented  at  the  M.I.T.  Lincoln  Laboratory  I Ire  pond  Infrared 
Research  Facility  at  Weetlord,  MA.  The  filter  estimates  a six  dimensional  elate  vector  for  mount  direction  from  satellite  ob- 
servations. These  observations  can  consist  of  range,  elevation,  axlmuth,  and  range  rate;  and  under  push-button  control,  can 
be  selected  from  among  the  available  Klrepond  detectors  (IR  and  optical)  and  the  Millstone  radar  across  the  road. 

Radar  polar  coordinates  are  used  throughout  and.  In  particular,  both  the  estimates  and  the  equations  of  motion  are  In  these 
coordinates.  The  filter  la  fully  coupled  In  the  aenae  that  every  measurement  Improves  every  estimate.  For  example,  angle 
measurements  Improve  range  and  Doppler  estimates,  and  conversely.  Serial  processing  of  simultaneous  measurements  Is 
employed.  ThU  eliminates  the  need  for  matrix  Inversion,  facilitates  handling  of  missing  data  points,  r<  tires  less  storage, 
and  is  computationally  faster. 

A detailed  mathematical  description  of  the  filter  la  Included  along  with  some  typical  satellite  tracking  results,  jk 
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